#pragma once
#include "Road.h"
#include "Junction.h"
#include <unordered_map>
#include "../tools/tinystr.h"
#include "../tools/tinyxml.h"
namespace OpenDrive {
	struct keyPoint {
		float x, y;
	};
	struct Position {
		Position(std::string roadId, int laneSectionId, int laneId, float offset) :roadId(roadId), laneSectionId(laneSectionId), laneId(laneId), offset(offset) {}
		Position() {};
		std::string roadId;
		int laneSectionId;
		int laneId;
		//offset 沿着车道线的行驶方向从0-length
		float offset;
	};
	struct Edge {
		Edge(int id) :id(id), next(nullptr) {};
		//标识道路的唯一id（uniqueId）
		int id;
		Edge* next;
	};
	struct Node {
		Node() {};
		Node(std::string roadId, int laneSectionId, int laneId) :roadId(roadId), laneSectionId(laneSectionId), laneId(laneId), next(nullptr) {};
		//int id;
		std::string roadId;
		int laneSectionId;
		int laneId;
		Edge* next;
	};
	class OpenDrive
	{
	public:
		OpenDrive();
		OpenDrive(std::string odrvfileName);
		~OpenDrive();
		bool valid();
		void clear();
		bool open(std::string odrvfileName);
		bool findPath(Position start, Position end, bool append, bool turnRound = false);
		bool findPath(float x1, float y1, float x2, float y2, bool append, bool turnRound = false);
		bool findPath(keyPoint startp, keyPoint endp, bool append, bool turnRound = false);

	private:
		//输出文件路径前缀
		std::string prefix_output_path;
		//间隔多少米取一个点
		float stepLength;
		//车道总数
		int	laneNum;
		//保存道路连接关系
		std::vector<Node> nodes;
		//保存无法通过的道路位置
		std::vector<Position> invalidPosition;
		//tinyxml是读取解析xml的类
		TiXmlDocument *doc;
		//路径点xy是计算出的，z是固定值
		float Z_;
		//是否被初始化
		bool vaildState;
		bool debug;
		std::unordered_map<std::string, Road> roads;
		std::unordered_map<std::string, Junction> junctions;

		void		addRaod(const Road &road);
		Road &		getRoad(std::string roadId);
		bool		roadExist(std::string roadId);
		void		addJunction(const Junction &junction);
		std::string getLaneType(std::string roadId, int laneSectionId, int laneId);
		std::string getLaneType(int uniqueId);
		float		getLaneLength(std::string roadId, int laneSectionId, int laneId);
		float		getLaneLength(int uniqueId);
		bool		laneExist(std::string roadId, int laneSectionId, int laneId);
		Lane&		getLane(std::string roadId, int laneSectionId, int laneId);
		Lane&		getLane(int uniqueId);
		int			getLaneUniqueId(std::string roadId, int laneSectionId, int laneId);
		Position	findPosition(float x, float y);
		void		outputPointsToFile();
		void		outputRefPointsToFileByNum();
		void		format(int uniqueId);
		void		print();

		void calOneSideLine(LeftCenterRight &lr, bool left, const std::vector<Point>& refPoints, float startS, float endS);
		bool calOffset(const std::vector<Point>& refPoints, int startIndex, int endIndex, std::vector<Point>& res, int startIndex2, float a, float b, float c, float d, float offset, bool reverse, bool left, float threshold);
		void calOffset(const std::vector<Point>& refPoints, int startIndex, int endIndex, std::vector<Point>& res, int startIndex2, float a, float b, float c, float d, float offset, bool reverse, bool left);
		int  findIndex(const std::vector<Point>& refPoints, float s, bool reverse, bool start);
		//void calOffsetWidths(std::vector<Point>& points, int startIndex, int endIndex, float offset, float a, float b, float c, float d, std::vector<float>& offsetWidths, bool assign = false);
		void calLaneLineAndCenterLine();
		bool buildLaneLinkBetweenTwoLane(std::string startRoadId, int startLaneSectionId, int startLaneId, std::string endRoadId, int endLaneSectionId, int endLaneId);
		bool buildOneSideLink(std::string roadId, int laneSectionId, bool left);
		bool buildLaneLinkBetweenTwoRoad(std::string incomingRoadId, std::string connectingRoadId, std::string contactPoint, int from, int to);
		bool AstarPlan(Position start, Position end, bool append, bool turnRound = false);
		bool buildMap();
		bool parseOpenDriveFile();
		void parseLeftCenterRight(LeftCenterRight & LCF, TiXmlElement * lanes, std::string roadId, int laneSectionId, float s);
	};
}